

#requires the robot team module
require 'control_lib/level1/main/robot.rb'

#requires the robot robotmap module
require 'control_lib/level1/support/robotmap.rb'

#requires the robot log module
require 'control_lib/level1/support/robotlog.rb'


#require the math library
def de2rad(float)
  rad=float
  pi=3.14159265
  if (rad>0)
    return ((2*pi)/360)*rad
  else
    return 2*pi+(((2*pi)/360)*rad)
  end
end

################ PROGRAM STARTS HERE ########################

#start the robot
robot=Robot.new("10.1.157.59",3000)
#robot.init("USARBot.P2AT","-6.0,-2.0,1.0","robot1")
robot.init("USARBot.P2AT","0.748, -7.282, -0.446","robot1")

robot.recievestart
sleep(2)

#start the robot logging
robotlog=RobotLog.new(robot,true)
robotlog.reset_logs 
robotlog.startlog

#start the map
map=Robotmap.new(robot)
map.configbeliefmap(600,600,0.11,0.11)
map.createbeliefmap
map.autoupdatemap(true)
map.autologmap(true)

robot.colguardmode
#end of init

cbot=robot

#Main Loop
oldtime=Time.now
totaltime=0.0
checkno=20
while(totaltime<=60.0*10.0)

  puts "totaltime:#{totaltime}"
  
  #start by backing up a bit
  cbot.smartback(0.2,2.0) if (cbot.col_frontcheck)

  #find max 
  scans=cbot.rangescan.size
  maxscans=Array.new()
  maxdeg=Array.new()
  scanarray=Array.new(cbot.rangescan)

  checkno.times{
  max=0.0
  index=-1
  scanarray.each_with_index{|rangestr,deg|
    range=rangestr.to_f
    if(range>max and range<19.9)
      max=range
      index=deg
    end
   }
  scanarray.delete_at(index) #remove from list
  maxscans<<max
  maxdeg<<(index)
  }

  #create roulette wheel
  roullet=Array.new(maxscans)

  #find sum
  sum=0.0
  roullet.each{|i|
    sum=sum+i
  }

  #change to percents 
  roullet.collect!{|member|
    member=(member/sum)*100
  }

  #change to sumation of percents
  sum=0.0
  roullet.collect!{|member|
    member=member+sum
    sum=member
  }

  #randomize and choose
  choice=rand(100)
  choicedeg=0.0
  roullet.each_with_index {|member,index|
      if(choice<member)
        choicedeg=maxdeg[index]
        choice=maxscans[index]
        break
      end
  }
  #the chosen deg is choicedeg and distance is choice
  puts choice,choicedeg 
  #back up a bit
  puts de2rad(90-choicedeg)

  if(choicedeg<=90)
    cbot.smartright(de2rad(90-choicedeg))
  else
    cbot.smartleft(de2rad(choicedeg-90))
  end

  while(cbot.fSONARSEN[6].to_f<0.5)
       cbot.smartleft(0.25)
       puts "co: #{cbot.fSONARSEN[7].to_f}"
    end
   while(cbot.fSONARSEN[2].to_f<0.5)
       cbot.smartright(0.25)
       puts "co: #{cbot.fSONARSEN[1].to_f}"
    end
  cbot.smartfor(1.0,2.0)
  sleep(1.0)

end  #while
sleep(20)